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ABSTRACT 


The  U.S.  Navy  and  a  number  of  its  contractors  are 
presently  developing  unmanned  miniature  submarines  for 
several  vital  underwater  missions.  These  include  surveil¬ 
lance,  submarine  tracking,  and  bottom  mapping.  Foregoing 
Reserarch  at  NPS  produced  a  '’testbed"  as  a  research  platform 
for  demonstrating  the  performance  of  AUVs.  Combining  the 
power  of  an  IBM  PC/AT  in  conjunction  with  a  high  level 
programming  language,  a  state  space  dive  control  system  was 
developed  and  instituted  for  the  30  inch  AUV  model. 
Parameter  Estimation  using  a  Recursive  Least  Squares  Fit 
scheme  and  a  State  Observer  were  incorporated  in  the 


controller. 


Procedures  dealing  with  hardware/software 


interfacing,  AUV  simulation  analysis,  and  computation  speed 
of  large  programming  code  were  investigated.  '/  -•  • 
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INTRODUCTION 


I . 


A.  GENERAL 

The  crux  of  this  thesis  is  to  develop,  test  and  evaluate 
a  real-time  state-space  derived  dive  plane  controller  for 
the  NPS  prototype  Autonomous  Underwater  Vehicle  (AUV) . '  The 
U.S.  Navy  and  a  number  of  its  contractors  are  presently 
developing  unmanned  miniature  submarines  (AUV's)  for  several 
important  underwater  missions.  These  include  surveillance, 
submarine  tracking,  submarine  decoys,  bottom  surveying,  and 
off-board  sensor  support  for  both  surface  ships  and 
submarines  [Ref.  1],  The  shape  of  these  vehicles  is  similar 
to  that  of  a  large  torpedo  and  can  be  deployed  on 
submarines,  and  surface  ships. 

The  AUV  testbed  at  the  Naval  Postgraduate  School  (NPS) 
currently  being  developed  is  to  examine  the  use  of,  and 
problems  associated  with,  advanced  control  technologies. 
The  objectives  include: 

-  determine  requirements  imposed  by  the  implied  need  for 
artificial  intelligence, 

-  development  of  advanced  computer  control  concepts  and 
architecture , 

-  determine  system  interface  feasibility.  [Ref.  2] 

Unlike  the  conventional  body-of-revolution  design,  the 

NPS  testbed  is  similar  to  a  low  aspect  ratio  wing.  It  is 


similar  conceptually  to  the  late  model  Swimmer  Delivery 


Vehicle  (SDV)  design,  but  actually  based  on  a  two  and  three 
to  one  aspect  ratio  box  shape  vehicle.  Vehicle  selection 
was  based  initially  on  the  availability  and  thoroughness  to 
which  the  vehicle  hydrodynamic  characteristics  were  modeled 
and  validated,  providing  for  ease  of  program  development  and 
verification  [Ref.  3]. 

Modern  trends  in  control  system  design  are  moving 
towards  greater  complexity.  The  desire  for  complex  tasks 
and  greater  accuracy  fuel  the  need  for  high  powered 
computers  to  accomplish  real-time  control  tasks  in  short 
duration.  One  of  the  main  interests  here  is  to  determine 
the  capabilities  of  real-time  control  with  adaptive 
autopilots  and  various  computer  architectures.  The  need  for 
adaptivity  lies  in  the  need  to  achieve  robust  performance 
over  a  wide  range  of  operating  conditions.  Also,  although 
computational  speed  may  suffer,  rapid  response  is  seen  to  be 
important  when  evasive  and/or  obstacle  avoidance  maneuvers 
are  called  for  at  the  supervisory/expert  system  level.  It 
follows  that  the  design  trade-off  between  response  and 
robustness  requires  examination.  For  these  reasons,  this 
particular  work  has  concentrated  on  the  development  of  a 
real-time  controller  including  the  ability  to  perform 
parameter  estimation,  and  the  issues  surrounding  its 
response  time  performance. 


2 


B.  REVIEW  OF  PREVIOUS  WORK 

The  initial  proposal  from  the  Naval  Postgraduate  School 
for  research  on  an  Autonomous  Underwater  Vehicle  began  1 
October  1987.  It  was  titled  "Navigation,  Path  Planning, 
Dynamics  and  Control  of  Generic  Autonomous  Underwater 
Vehicles,"  and  was  submitted  to  the  Naval  Surface  Weapons 
Center  in  Silver  Spring,  Maryland.  The  primary  stated 
objective  was  to  assess  the  capabilities  for  the  design, 
construction,  testing,  and  operation  of  a  testbed  as  a 
research  platform  for  demonstrating  the  performance  of  AUVs 
[Ref.  2]. 

The  first  task  involved  selecting  a  suitable  submersible 
model.  A  non-conventional  wing-shaped  model  with  body  cross 
sections  nearly  rectangular  rather  than  circular  became  the 
selected  choice. 

Using  Dynamic  Simulation  Language  (DSL),  Boncal  (1987) 
modified  the  existing  equations  of  motion  ( DTNSRDC  2510)  and 
performed  all  the  necessary  differentials  for  their  lineari¬ 
zation  to  suit  the  needs  of  the  AUV  controller  development. 
The  linearization  was  necessary  to  the  design  of  model-based 
controls  and  to  simulate  controlled  responses  for  all  six 
degrees  of  freedom  of  vehicle  motion  [Ref.  3].  Both  linear 
and  non-linear  models  were  developed  and  evaluated.  This 
lead  to  the  initial  development  of  a  controller  robust 
enough  to  handle  a  variety  of  reflexive  type  maneuvers  over 
a  wide  range  of  speeds  (in  the  programming  language  DSL). 
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In  order  to  investigate  the  real-time  control  issues, 
and  to  provide  a  means  to  generate  experimental  data  for  the 
evaluation  of  parameter  identif ication  methods,  a  later 
effort  by  Brunner  (1988),  set  out  to  design,  build,  and  test 
a  model  30  inches  in  length  and  7  inches  wide.  The  model 
was  self-propelled  and  remotely  controlled  via  radio 
transmission.  It  was  equipped  with  rate  gyro  sensors,  a 
pressure  cell  depth  sensor  and  a  pressure  cell  speed  sensor. 
Two  DC  motors  provided  the  propulsion  power  [Ref.  4].  Open- 
loop  dive  plane  dynamic  response  tests  were  performed  in  a 
water  tank  where  response  measurements  of  depth  were  made  by 
pressure  cells,  and  pitchrate  by  a  pitchrate  gyro.  Data 
from  each  run  were  recorded  using  PCLAB  and  the  DT  NOTEBOOK 
[Ref.  5]  data  acquisition  software,  and  analyzed  using  the 
MATRIXx  [Ref.  6]  data  analysis  capability.  Comparing  these 
data  with  the  computer  model  simulation,  hydrodynamic 
coefficients  and  system  transfer  functions  were  developed. 

Initial  developments  of  a  digital  autopilot  to  support 
the  NPS  test  vehicle  were  made  by  Delaplane  (1988) ,  wherein 
an  I  PM  PC/AT  was  used  in  conjunction  with  DATA  TRANSLATION 
Analog  to  Digital  (A/D)  interfacing  boards,  as  the  platform 
for  the  real-time  closed  loop  controller.  A  simplified 
control  program  for  AUV  dive  plane  commands  had  been 
implemented.  Signal  generators  were  used  to  replicate  AUV 
transducer  outputs  of  depth,  speed,  and  pitchrate  to  verify 


system  operation  [Ref.  7].  The  inclusion  of  real  control 
laws,  and  the  system  parameter  identification,  however,  was 
left  to  the  activity  reported  herein. 

C.  SCOPE  OF  WORK 

The  scope  of  this  investigation  was  to  develop  the 
digital  autopilot  [Ref.  7]  into  a  three  state  feedback, 
Single  Input  multiple  Output,  real-time  closed-loop  dive 
control  system  for  the  NPS  AUV  model.  Initial  stages  of 
research  lead  to  the  notion  that  the  pitch-heave  coupling 
prevalent  in  many  underwater  vehicles  was  not  significant 
for  this  vehicle.  The  three  state  variables  of  the  dynamic 
model  were  then  reduced  to  the  pitch  rate,  pitch  angle,  and 
the  water  depth.  Because  of  tank  length  limitations  it  was 
foreseen  that  an  analog  computer  model  of  the  vehicle 
dynamics  would  be  essential  to  the  ease  of  the  real  time 
control  development,  and  therefore,  a  scaled  simulator  was 
built. 

The  final  stages  of  program  development  were  to 
implement  the  digital  dive  plane  controller  compatible  with 
the  30  inch  AUV  model.  Considering  the  small  vehicle  size, 
and  relatively  high  component  costs,  the  NPS  AUV  was  unable 
to  include  a  pitch  angle  sensor.  Since  pitch  angle  feedback 
was  later  shown  to  be  critical  to  the  autopilot  stability, 


where 
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pitch 

angle  could 

not  be  measured 

directly,  a 

state 

reconstruction  system 

was  needed 

and 

had 

to  be 

designed . 

This 

led  to  the 

development 

of  a 

full 

state 
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observer  driven  by  measured  inputs  and  outputs  [Ref.  8]. 
The  Observer,  including  one  identification  of  pitch  rate 


sensor  bias,  the  bias  compensation,  and  its  real  time 
performance  was  large  part  of  this  effort. 

D.  APPROACH 

Primarily,  a  solid  understanding  of  the  software/ 
hardware  used  was  essential.  In  this  control  system,  a  high 
level  programming  language  (Turbo  Pascal)  was  interfaced 
with  an  A/D  translation  board  (hardware)  via  a  software 
package  (PCLAB) .  References  9,  10,  11  established  a 
sufficient  base  on  which  the  elaborate  program  schemes  could 
be  developed  effectively.  Once  accomplished,  the  challenge 
of  debugging  large  programs  could  readily  be  resolved. 

Secondly,  incorporating  experimental  data  from  previous 
AUV  research  proved  paramount.  This  research  thesis  could 
not  be  accomplished  without  the  foundation  set  by  preceding 
studies . 

Lastly,  using  real-time  feedback  of  AUV  model  dynamics 
via  the  analog  computer  strengthened  the  ease  by  which 
program  development  could  proceed.  The  Analog  Computer 
simulated  the  model  dynamics  including  the  sensor  bias  and, 
partially,  the  effects  of  changing  speed.  This  provided  for 
the  preliminary  design  of  a  robust  state-space  controller. 
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E.  WHAT  FOLLOWS 

In  Chapter  II,  some  detail  is  provided  concerning  the 
models  of  the  vehicle  dynamics.  Chapter  III  discusses  the 
analog  computer  modelling.  Chapter  IV  gives  an  account  of 
the  program  development.  Chapters  V  and  VI  contain  a 
discussion  of  results  and  Chapter  VII  is  a  summary  and 
conclusion . 
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II .  SIMPLIFIED  MODELING  OF  VEHICLE  DIVE  DYNAMICS 

A.  GENERAL 

In  this  chapter,  a  review  of  earlier  experiments 
conducted  under  the  work  of  Brunner  [Ref.  4],  appears  and 
the  details  of  the  third-order  model  of  the  vehicle  is 
given.  This  leads  to  a  discussion  of  the  design  issues  in 
the  controller  and  the  observer  systems  and  the  special 
problem  of  identifying  the  pitch  rate  sensor  bias  from  input 
and  output  data. 

B.  REVIEW  OF  OPEN  LOOP  EXPERIMENTS 

Open  loop  experiments  were  conducted  during  January, 
1988  (Brunner)  ,  in  which  the  model  AUV  was  driven  by  open 
loop  command  to  perform  dive  plane  maneuvers.  From  these 
test  runs,  MATRIXx  datafiles  were  constructed  for  both  the 
command  voltage  to  the  dive  plane  and  the  measured  voltages 
corresponding  to  the  pitch  rate,  and  the  water  depth 
responses.  Using  the  linear  model  time  history  response 
features  in  MATRIXx,  together  with  many  simulation  runs  in 
which  hydrodynamic  parameters  were  varied  both  in  value  and 
in  combinations  of  values,  several  "best  fit"  parameter 
combinations  v  're  synthesized  to  represent  the  dynamics  of 
the  model  AUV.  It  was  discovered  that  the  influence  of 
coupling  between  the  heave  mode,  and  the  pitch  mode  dynamics 
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was  not  strong  in  determining  the  pitch  rate  behavior,  and 
that  for  the  purposes  of  control  shape  design,  the  heave 
motion  would  be  neglected.  The  influence  of  the  power  cord 
was  found  to  be  a  determining  factor  in  the  overall  heave 
response,  but  future  tests  would  be  conducted  by 
neutralizing  the  weight  of  the  cord,  and  this  effect  would 
not  adversely  affect  closed  loop  results. 

The  numerical  values  of  the  vehicle  parameters,  with 
their  respective  dimensional  units,  were  established  as 
follows  in  the  next  section. 

C.  THIRD-ORDER  MODEL  FOR  DIVE-PLANE  DYNAMICS 

The  vehicle  equations  of  motion  in  the  dive  plane  are 
the  surge,  heave,  and  pitch  motion  equations  together  with 
the  kinematical  relationships  linking  the  global 
longitudinal  and  vertical  velocity  components  to  the  body 
fixed  surge,  heave,  and  pitch  rates.  These  are  given  in 
detail  in  Boncal  (1987) .  When  linearized  about  a  constant 
speed  straight  line  flight  path,  the  relationships  simplify 
to  four  first-order  state  equations  in  the  heave  and  pitch 
rates  and  positions.  In  short, 

ZW*UX*W  ZQ*UX*q  ZD*UX2*6  Fl 

W  "  MZDW  MZDW  L*MZDW  MZEW 

•  __  M*UX*W  MQ*UX*q  1T)*UX2*6  F2 

q  l2*iyy  l2*iyy  ^ 

=  q 
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z  =  w  -  UX*' 


where  w,  q,  f  ,  z  are  the  four  state  variables  and  the 
parameters  are  given  by, 


HEAVE 

DAMPING 

ZW 

= 

-1.5 

PITCH 

CROSS  COUPLING 

ZQ 

= 

0.0 

HEAVE 

CROSS  COUPLING 

MW 

= 

0.0 

PITCH 

DAMPING 

MQ 

= 

-0.15 

MOMENT 

EFFECT 

ZD 

= 

0.0 

MOMENT 

EFFECT 

MD 

= 

0.225 

MASS  PLUS  ADDED  MASS 

MZDW 

= 

1.005 

INERTIA  PLUS  ADDED  MASS 

IYY 

= 

0.0  72 

EFFECT 

OF  TETHER 

FI 

= 

0.11 

EFFECT 

OF  TETHER 

F2 

= 

0.0 

FORWARD  SPEED 

UX 

= 

2.1  FEET/SEC 

LENGTH 

L 

s 

2.5  FEET, 

where  the  units  of  the  parameters  are  given  in  Table  2.1. 


TABLE  2.1 
PARAMETER  UNITS 


Parameter 

Units 

Value 

( Dimensional ) 

Value 

fVolts  Units} 

MQ*UX/L*IYY 

sec"^- 

10.0  1/sec 

• 

00 

UX 

ft/sec 

10.0  ft/sec 

2.1 

MD*UX2/L2IYY 

rad/sec2 

10.0  1/sec2 

2 . 531 

10 


It  should  be  pointed  out  that  the  matching  of  experimental 
data  was  done  by  comparison  of  sensor  output  voltage 
response  time  history  and  that  proper  conversion  to 
dimensional  values  was  essential  to  being  able  to  translate 
to  control  gains  for  real  time  control  system  design. 

Because  it  was  found  that  the  effects  of  the  heave/pitch 
coupling  were  not  strong,  and  that  the  influence  of  w  was 
dominated  by  the  power  cord,  it  was  assumed  that  for  the 
purpose  of  control  system  design,  the  heave  dynamics  would 
remain  unmodelled,  and  the  model  base  would  be  reduced  to 
third-order  with  the  equations  as  follows: 


x'  =  [q,e ,Z] , 


—  — 

—  — 1 

-1.8  0.0  0.0 

2.531 

1.0  0.0  0.0 

X  + 

o 

o 

0.0  -2.1  0.0 

o 

o 

L  — i 

_ 

D.  FULL  STATE  FEEDBACK  CONTROL  DESIGN 


For  the  nominal  forward  speed  of  2.1  ft/sec,  the  system 
dynamics  matrix,  and  the  system  input  and  output  matrices 
are : 


00 

0.0 

0.0 

2.531 

A  = 

1 . 0 

0.0 

o 

o 

B  = 

0 . 0 

^  o 

o 

-2.1 

0.0_ 

1 

o 
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These  equations  are  characterized  by  the  open  loop 


eigenvalues : 


v  =  [-1.8, 0.0, 0.0] 


which  properly  indicates  the  first-order  pole  for  the  pitch 
rate  response  and  the  double  integration  from  pitch  rate  to 
depth.  The  output  matrix  has  diagonal  coefficients  with 
either  unity  values  or  other  constants  depending  on  whether 


the  output 

signals  are  considered 

to 

be 

sensor 

output 

voltages  or 

the  dimensional  values 

of 

the 

system 

state 

variables . 

Noting  that  the  open  loop  system  is  completely 
controllable  in  the  formal  sense,  a  pole  placement  technique 
was  determined  to  be  appropriate  for  the  autopilot  design. 
Additionally,  since  the  overall  system  required  a  digital 
controller,  discrete  time  methods  were  needed,  and  the 
question  of  sample  rate  arose. 

Digital  systems  for  the  control  of  continuous  processes 
are  limited  to  providing  a  sequence  of  stepwise  constant 
levels  that  can  be  updated  every  sample  time,  but  remain 
fixed  within  that  interval.  Clearly,  a  short  interval  is 
preferred.  However,  the  execution  time  of  extensive  amounts 
of  code  requires  that  the  sampling  period  must  be  sufficient 
for  code  execution,  but  it  must  not  be  so  short  as  to 
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places  limitations  on  the  closed  loop  poles  as  specified  for 
the  control  design,  being  consistent  with  the  execution  time 
of  the  real  time  code. 

In  balancing  the  foregoing,  two  systems  were  designed. 
The  first,  at  20  Hz,  was  designed  to  accommodate  a  control 
code  that  executed  a  full  three  state  feedback  design  where 
the  system  parameter  identification  was  bypassed.  The 
second  was  at  5  Hz  (the  maximum  upper  limit  possible  under 
the  present  hardware  configuration)  where  parameter 
identification  was  performed. 

The  open  loop  system  dynamics  matrix  and  input  matrix 
were  then  discretized  using  the  20  Hz  sample  rate  so  that 
the  new  A,  and  B  matrices  became: 


.  9139 

0.  0 

0.  0 

.  121 

A  = 

.0478 

1.0 

0.0 

B  = 

.0031 

-.0025 

-.1050 

1.0 

-.0001 

Closed  loop  poles  for  the  20  Hz  system  were  then  specified 
at 


s  =  [0.92,0.925,0.926] 

in  the  z-domain,  corresponding  to  three  poles  in  the  s-plane 
with  time  constants  equal  to  1.0  sec,  with  the  following 
state  feedback  gains, 
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Kc*x 


[0.3066,0.8604,-0.2493], 


where  allowances  were  made  for  the  output  transducer  gain. 
For  a  state  regulator  system,  the  control  law  is, 

u  =  -Kc*x 

when  constant  non-zero  command  signals  are  to  be  introduced, 
the  control  law  requires  modification  to, 

u  =  N*r  -  Kc*x 

in  which  the  command (s)  r  and  the  set  point  scaling  matrix, 
N,  are  specified  so  that  the  tracking  error  r-y  vanishes. 
The  closed  loop  system  equations  become 

x  =  (A-B*Kc) *x+B*N*r;  y  =  C*x 

and  the  tracking  error  vanishes  in  steady  state  when, 

C* ( A-B*Kc) -1*B*N=I ;  N=(C* (A-B*Kc) -1*B) 

yielding  the  discrete  time  control  law  used, 
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u(k)  =  -0. 2493*[r(k) -z (k) ]-z (k) ] 

-0 . 8604  * [  (k)  ] -0. 3066* [q(k)  ] 

This  control  provides  reasonable  response  time  and  calls  for 
a  control  surface  activity  level  that  would  exceed  the 
stroke  of  the  vehicle  dive  plane. 

E.  OBSERVATION  OF  UNMEASURED  STATES 

In  the  development  of  the  model  AUV  the  size  limitation 
prevented  the  inclusion  of  an  angle  sensor  for  pitch,  so  the 
control  system  was  required  to  reconstruct  that  signal  in 
order  to  yield  stable  results.  The  application  of  a  full 
state  observer,  configured  as  a  model  based  compensator  is 
given  by  the  following  one  step  prediction  equations, 

x  ( k+ 1 )  =  A*x  (k)  +  B*u  (k)  +  Ko*  [y  (k)  -C*x  (k)  ] 

in  which  y(k)  is  the  previous  time  sensor  output  signals. 
In  this  case,  two  outputs  were  available  to  the  observer, 
the  pitch  rate,  and  the  water  depth.  The  observer  gains, 
Ko,  may  be  selected  by  using  the  dual  of  the  controller  pole 
placement  technique,  or  by  using  Linear  Quadratic  Regulator 
optimization.  Although  pole  placement  techniques  in  general 
are  based  on  a  single  input  signal,  two  inputs  can  be 
handled  by  the  assignment  of  some  weighting  value  (i.e., 
equal)  ineffective  towards  the  outcome.  The  effect  of  the 
two  signals,  y(k),  now  becomes, 
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y(k)  =  [ 1 ; 1] *ye (k) 


so  that  the  gains  for  the  two-input  system  become 

Ko  =  [ 1 ; 1 ] *ke 

where  Ke  are  found  via  pole  placement  using  ye(k). 

The  inclusion  of  the  observation  (and  later  parameter 
identification)  algorithms  required  the  use  of  a  slower 
sample  rate,  5  Hz.  Consequently,  enclosed  loop  poles  of  the 
observer  were  chosen  as  a  compromise  between  fast  response 
and  the  resulting  sensitivity  to  extraneous  noise  when 
implemented  as  a  model  based  compensator.  The  following 
were  the  final  selection, 

Ko  =  [0.0;-0. 4029;0.5677] 

F.  PARAMETER  ESTIMATION  OF  SENSOR  BIAS 

Parameter  identification  of  the  rate  gyro  bias  was 
accomplished  by  the  use  of  an  ARMA  model  of  the  gyro  system 
itself  where  the  input  variable  was  the  dive  plane  angle  and 
the  output  was  pitch  rate.  The  ideal  transfer  function 
between  input  and  output  is  a  first  order  system  which  has 
two  numerator  coefficients,  and  two  denominator  coefficients 
to  identify.  (Note  that  the  z-domain  transfer  function  is 
to  be  used  because  we  are  dealing  with  sequences  of  sampled 
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data.)  With  this  form,  the  parameter  vector  and  the  data 
vector  can  be  written  as, 

.  =  [1  al  bO  bl];  x'  =  [y(t)  y(t-l)  d(t)  d(t-l)]; 

and 


*x  =  bias  (t) . 

The  foregoing  is  then  repeated  and  a  least  squares  fit 
for  v  is  then  obtained  after  the  bias  is  included  inside  the 
vector  .  Least  squares  solution  of  these  equations  yields 
both  the  vector  of  parameters,  v  ,  and  the  unknown  bias.  In 
this  work,  a  recursive  least  squares  algorithm  was  used  with 
a  Householder  routine  to  compute  the  recursive  gains  which 
is  time  consuming  but  robust. 
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Ill .  ANALOG  SIMULATOR 

A.  GENERAL 

Simulation,  a  must  for  prototype  development,  provides  a 
source  of  numeric,  kinematic  and  logical  awareness  which  is 
vital  in  designing  a  complex  vehicle  system  and  analyzing 
its  performance.  It  provides  an  environment  in  which  the 
many  possible  states  of  the  system,  and  the  transitions 
between  them,  can  be  exercised  and  observed  in  an  accessible 
and  controlled  manner.  Properly  utilized,  simulation  will 
reduce  significantly  the  amount  of  field  testing  required. 
Design  and  performance  deficiencies  can  be  identified  early 
in  the  development  cycle,  before  the  system  is  deployed 
[Ref.  12]. 

A  technique  known  as  "Hybrid"  simulation  employs  a 
combination  of  analog  and  digital  computing  [Ref.  13]. 
Using  the  hydrodynamic  coefficients  of  the  AUV  model,  the 
analog  computer  can  be  "programmed"  to  simulate  AUV  vehicle 
dynamics.  With  the  control  program  running  and  a  "target 
depth"  entered  as  a  keyboard  input,  AUV  pitvh  angle,  depth, 
and  pitchrate,  calculated  by  the  analog  computer,  are  fed 
back  to  the  digital  computer.  The  three  states  are  compared 
to  a  model  reference  and  an  appropriate  dive  plane  command 
is  determined.  The  result  is  a  continuous  update  (real-time 


control)  of  vehicle  dive  plane  angle  (see  Figure  3.1  tor 
hardware  configuration). 

In  the  second  phase  of  program  development,  the  analog 
computer  was  configured  to  sustain  a  constant  or  variable 
bias  on  the  system.  This  lead  to  the  development  of  a  bias 
estimator . 

B.  DESCRIPTION  OF  ANALOG  SIMULATOR 

In  developing  the  analog  simulations,  the  equations  of 
motion  of  the  vehicle  are  required  to  be  converted. 
Convenient  conversion  or  scaling  is  based  on  the  maximum 
variable  values  being  scaled  with  a  +/-  10  volts  maximum 
output  on  each  amplifier.  In  analog  simulations,  variable 
summations  and  integrations  are  performed  by  summing  and 
integrating  operational  amplifiers.  Multiplication  by 
constant  coefficients  are  performed  by  attenuating  potenti¬ 
ometers  set  to  the  appropriate  fraction  of  1.0.  With 
scaling  consistently  applied  through  the  1  or  10:1  input, 
appropriate  gains  are  sent  to  the  corresponding  amplifiers. 

Figure  3.2  shows  an  example  of  a  first  order  system 
simulation  using  an  analog  operational  amplifier.  The 
equation  is: 


q  =  [ 1 . 5 ]  6  rad/sec 
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PITCH  ANGLE  0 


Figure  3.1  Hardware  Configuration 
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Figure  3.2  Example  of  a  First  Order  System 

Using  an  Analog  Operational  Amplifier 

where  5  is  in  radians.  Note  the  coefficient  1.5  rad/sec  per 
radian  is  set  at  0.15  in  the  potentiometer  with  a  gain  of  10 
in  the  integrating  amplifier. 

In  Figure  3.3,  the  diagram  represents  the  third  order 
model  of  the  vehicle  given  by  Equations  in  II. C.  Table  2.1 
gives  a  listing  of  the  system  parameter  values,  their 
dimensions,  and  the  potentiometer  settings  used  in  the 
simulations. 
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Figure  3.3  Vehicle  Simulation  Analysis 
Using  an  Analog  Computer 
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IV.  DIGITAL  CONTROL  PROGRAM  DEVELOPMENT 

A.  GENERAL 

As  explained  in  Reference  7,  this  digital  control 
program  represents  a  functional  component  at  the  lowest 
level  of  the  digital  autopilot  echelon.  The  digital  control 
program  interfaces  with  the  servos  controlling  the  AUV  dive 
planes.  The  "handshaking"  of  data  between  the  digital 
controller  and  the  voltage-operated  servos  onboard  the  AUV 
model  are  accomplished  by  A/D  interface  hardware  [Ref.  14]. 
In  Figure  3.1  the  hardware  arrangement  using  the  analog 
computer  as  a  simulator  of  sensor  feedback  is  exhibited. 

Much  of  the  program  development  of  Reference  7  is 
implemented  into  this  thesis.  A  number  of  dilemmas  had  to 
be  overcome  for  program  progression  to  continue. 

The  preliminary  program  code  had  parameter  conversions 
of  feet/sec,  inches,  and  degrees/sec.  To  alleviate  valuable 
computation  time,  and  to  better  support  the  AUV  model,  the 
code  for  the  input  and  ouput  (I/O)  dimensions  was  modified. 
All  I/O  parameter  dimensions  were  converted  to  voltages. 

Significant  concern  focused  on  the  programmable  inter¬ 
rupt  control.  Previous  work  was  hampered  when  software 
interaction  was  unachievable.  The  preliminary  control 
program  would  run  only  when  initial  keyboard  inputs  of 
target  depth  were  entered.  Pre-programmed  keyboard 
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interrupt  modes  would  not  respond  to  subsequent  inputs. 
Keyboard  interaction  was  masked  and  additional  dive  plane 
commands  could  not  be  executed.  The  program  could  terminate 
only  if  the  CTRL-BREAK  key  combination  was  executed. 

From  Reference  11  it  was  discovered  that  by  removing  the 
influence  of  the  CTRL-BREAK  character  at  the  start  of  the 
programming  code,  programmed  interrupts  could  be  invoked. 

By  overcoming  this  impasse,  program  development  was 
inevitable.  Significant  algorithm  progression  followed. 
"Canned"  maneuvers,  invoked  by  keyboard  inputs,  were 
developed  and  tested.  Data  collection  and  filing  were 
implemented  by  actuating  function  key  inputs. 

The  final  phase  of  research  was  committed  to  the 
development  and  implementation  of  an  Estimator/Observer 
control  program.  This  algorithm  would  reconstruct  the  state 
variable,  pitch  angle,  using  depth  and  pitchrate  sensor 
feedback.  Bias  created  by  the  relatively  inexpensive 
onboard  sensors  was  determined  by  a  Kalman  filter  and 
removed  from  the  system.  A  cleaner,  more  robust  control 
signal  resulted. 

B.  A/D,  D/A  CONVERSION 

The  Analog  to  Digital  (A/D)  and  Digital  to  Analog  (D/A) 
conversion  process  is  vital  to  the  operation  of  this  control 
system.  Analog  sensor  signals  from  the  AUV  are  digitized 
via  the  A/D  process  prior  to  entering  the  computer  control 
program.  Likewise,  digitized  command  signals  are  converted 
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to  analog  signals  via  the  D/A  process  prior  to  return  to  the 
dive  plane  servos.  For  the  preliminary  AUV  autopilot  design 
described  in  Reference  7,  the  total  cycle  time  for  a 
complete  I/O  conversion  process  was  5  milliseconds  with  a  50 
millisecond  sample  rate  (20  Hz)  .  In  later  paragraphs, 
discussion  will  focus  on  the  need  to  decrease  the  sample 
rate  in  order  to  augment  the  larger,  more  complex  control 
code . 

The  digitizing  process  is  accomplished  by  the  Data 
Translation  (DT)  board,  DT  2801-A.  It  is  installed  internal 
to  the  computer  as  explained  in  Reference  7.  The  board 
configuration  can  be  found  in  the  technical  reference  manual 
[Ref.  14].  All  conversions  on  the  DT  2801-A  board  are 
controlled  by  writing  data  to,  and  reading  data  from, 
registers  on  the  board.  PCLAB  subroutines  is  an  interfacing 
software  package  that  coordinates  this  task  [Ref.  15] 

Conversion  of  a  voltage  value  to  a  digitized  value 
depends  on  the  number  of  bits  of  resolution  used  by  the 
converter.  For  instance,  a  12-bit  converter  has  4096  Number 
Of  Codes  (NOC)  .  For  a  bipolar  range  of  +/-  10  volts,  a  0 
NOC  would  correspond  to  -10  volts,  while  a  4096  NOC  would 
correspond  to  +10  volts.  An  NOC  of  2048,  similarly,  would 
correspond  to  0.0  volts. 

Before  these  digitized  voltage  values  are  usable  in  the 
software  control  program,  they  need  to  be  scaled  to  an 
equivalent  numerical  voltage  value.  Equations  found  in 
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Appendix  D  of  [Ref.  15]  were  implemented  into  the  control 
code  for  such  a  reason. 

C.  REAL-TIME  CONTROL  CODE 

The  control  program  code  was  written  in  the  programming 
language  Turbo  Pascal  version  3.0.  A  more  proficient 
version  (V4.0)  is  presently  available,  but  it  is  not 
currently  supported  by  the  interfacing  software  ( PCLAB) . 

A  real-time  controller  is  "real-time"  up  to  the  limits 
of  the  software  program  implementing  it.  A  significant 
advantage  of  Turbo  Pascal  is  operation  speed.  Reducing 
large  problems  down  into  smaller,  easier  defined  procedures 
affords  faster  execution.  Each  procedure  can  then  be 
subdivided  further,  at  the  discretion  of  the  programmer. 

The  control  program  executes  an  AUV  dive  control  system 
within  a  user  friendly  multiple  menu-shell  setup.  When 
running  the  compiled  control  program,  the  first  screen 
encountered  is  the  Main  Menu.  Here,  the  user  is  presented 
with  a  display  of  program  titles  and  the  options  to  quit  or 
continue  (run) .  The  next  screen  offers  a  similar  choice, 
where  depressing  the  function  key  FI  starts  the  control 
mode.  In  this  mode,  the  user  is  asked  to  name  a  "data  file" 
(standard  IBM  DOS  file) .  The  results  of  the  vehicle 
dynamics  during  the  course  of  the  run  will  be  stored  in  this 
file.  The  final  input  screen  prompts  the  user  to  "ENTER  THE 
TARGET  OPERATING  DEPTH."  Completion  of  this  procedure 
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activates  the  closed-loop  dive  plane  control.  The  Run-Mode- 
Screen  is  introduced  on  the  monitor  displaying  updated 
values  of  AUV  depth,  pitch,  pitchrate,  dive  command  angle, 
sensor  bias,  and  attitude. 

During  closed-loop  control,  the  user  has  four  methods  of 
operation : 

-  F3--exit  from  the  "Active  Control  Mode,"  and  file  close¬ 
out. 

-  F2 — activates  a  pre-programmed  "Canned"  maneuver. 

-  FI — exit  from  the  "Active  Control  Mode,"  close-out 
previously  selected  file,  creates  new  file  with  user 
interaction,  and  requests  new  target  depth  to  reactivate 
closed-loop  control. 

-  Any  alpha/numeric  key — temporarily  exits  from  "Active 
Control  Mode,"  prompts  user  for  a  new  target  depth, 
while  retaining  previously  selected  data  file  for  data 
storage . 

Two  separate  control  programs  were  used  to  establish 
data  collection  and  analysis.  The  first,  AUVCOP3F. PAS ,  was 
designed  as  a  closed-loop  dive  control  system  with  full 
state  feedback.  The  "C0P3"  corresponds  to  the  third  copy  or 
generation  control  program.  The  "F"  coincides  with  data 
Filing  capabilities.  "PAS"  is  Turbo  Pascal's  standard  file 
type  designation  for  programs  to  be  complied.  The  second 
program,  AUVC0P4F . PAS ,  implements  an  Observer/Estimator  to 
reconstruct  an  unknown  state  variable  from  two  sensor 
inputs.  Sensor  Bias  is  also  estimated  and  removed  from  the 
system. 
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D.  SAMPLE  RATE  CONTROL 

A  signal  generator  excites  the  DT  2801-A  board  to  pass 
data  to  and  from  the  data  registers.  The  dial  setting  of 
the  generator  determines  the  rate  at  which  this  is 
accomplished.  As  mentioned  earlier,  a  sample  rate  of  20  Hz 
was  used  in  Reference  7. 

When  running  the  full-rate  feedback  control  program 
(AUVCOP3F. PAS)  at  20  Hz  sample  rate,  Turbo  Pascal  computed 
the  relatively  small  algorithm  in  5  milliseconds.  Manually 
reducing  the  sample  rate  during  program  runtime  resulted  in 
a  stable  controller  as  low  as  3  Hz.  An  ideal  condition, 
though,  is  to  maintain  a  high  sample  rate  to  yield  a  more 
stable,  faster  responding  controller. 

The  Estimator/Observer  control  program  (AUVCOP4F. PAS) , 
however,  contains  an  additional  400  hundred  lines  of  code. 
Here,  computation  time  was  increased  significantly  (greater 
than  150  milliseconds)  and  could  not  keep  pace  with  the  20 
Hz  sample  rate.  To  accommodate,  the  sample  rate  was  reduced 
to  5  Hz.  Likewise,  new  gains  were  derived. 

Current  hardware/software  configurations  of  the  AUV 
controller  are  limited  in  performing  these  multiple  matrix 
calculations  at  high  sample  rates. 

E.  SOFTWARE  LIMIT  FOR  ACTUATOR  SATURATION 

Present  hardware  configurations  on  the  30  inch  model 
required  small  input  voltages  to  actuate  the  full  range  of 
the  dive  plane  angle.  During  previous  research  of  open-loop 
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tests,  dive  plane  angle  was  compared  with  the  corresponding 
input  voltage.  Results  concluded  that  a  linear 
proportionality  on  the  order  of  0.01  volts  per  degree  dive 
plane  angle  existed.  This  was  true  for  a  range  of  0.0  to 
10.0  degrees.  Hence,  the  operating  range  of  the  dive  planes 
became  +/-  10  degrees,  and  respectively,  the  operating 
voltage  range  became  +/-  0.1  (using  the  hand-held 
transmitter).  The  dive  plane  voltage  range  of  +/-  0.1  was 
implemented  into  the  analog  computer  configuration.  The 
control  code  was  changed  by  creating  a  software  stop  prior 
to  sending  the  final  command  signal  to  the  plane.  This  was 
implemented  by  limiting  the  final  signal  sent  through  the 
D/A  conversion  to  +/-  0.1  volts  in  the  form, 

if  abs(u)  >  o.l  then  u  =  0 . l*abs (u) /u . 

F.  FILES  FOR  DATA  RECOVERY 

During  the  open-loop  vehicle  tests,  data  acquisition  was 
achieved  by  using  the  software  program  DT  Notebook  [Ref.  5] 
on  the  IBM  PC/AT.  The  computer's  system  configuration  file 
had  to  be  specifically  structured  for  DT  Notebook  operation. 
However,  the  programming  language  Turbo  Pascal  used  an 
entirely  different  configuration.  The  two  programs, 
therefore,  could  not  be  run  simultaneously.  In  other  words, 
data  acquisition  by  DT  Notebook  could  not  be  performed  while 
running  the  closed-loop  control  program. 
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An  alternate  method  of  data  recovery  was  devised  using 
Turbo  Pascal  [Ref.  11].  The  keyboard  commands  for  data  file 
use  are  found  in  Chapter  IV. C.  Although  additional  through¬ 
put  is  needed  for  data  file  simulation,  the  time  used  in 
writing  data  to  a  hard  disk  was  found  to  be  negligible  in 
comparison  to  the  sample  rate. 


and  the  feedback  control: 


where  the  input  of  target  depth  is  the  set  point  r,  the 
closed  loop  gains  are  Kc,  and  the  setpoint  matrix  N  is 
designed  for  zero  steady  state  depth  error.  The  following 
sections  in  this  chapter  evaluate  the  response  of  the  above 
system  under  varying  conditions.  The  gains  are  fixed  and 
the  sample  rate  is  20  Hz. 

B.  RESULTS  OF  FULL  STATE  FEEDBACK 

In  the  full  state  feedback  controller,  the  closed  loop 
gains  are  designed  for  an  AUV  speed  of  2.1  Ft/s.  In  the 
following  figures,  AUV  depth  voltages  ranging  from  0.0  to 
10.0  volts  represent  the  water  depth  from  the  surface  to  a 
depth  of  10.0  feet  below  the  surface  respectively.  The  dive 
command  voltage  ranges  from  +/-  0.1  volts  equivalent  to  a 
dive  plane  angle  of  +/-  10  degrees.  In  Figure  5.2,  a  target 
depth  of  6.0  volts  becomes  the  new  set  point  from  1.0  volt. 
Subsequently,  a  dive  command  of  10  degrees  is  sent  to  the 
dive  planes.  Almost  immediately,  the  AUV  responds  and 
levels  out  at  the  new  depth.  The  entire  "run"  lasts 
approximately  120  sample  increments  or  six  seconds,  as 
expected,  with  an  overall  dominant  time  constant  close  to 
the  1.0  second  used  in  the  gain  design  process. 

C.  EFFECT  OF  ACTUATOR  SATURATION 

As  previously  mentioned,  the  dive  plane  actuator  is 
limited  to  a  range  of  +/-  10  degrees.  If  this  limit  is 
removed  then  the  results  of  Figure  5.3  are  found.  Comparing 
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this  to  a  second  run,  having  actuator  saturation,  the 
results  of  Figure  5.4  show  that  the  non-saturated  dive 
command  achieves  a  slightly  faster  AUV  depth  response. 
Figure  5.4  attains  5.0  volts  in  approximately  six  seconds 
whereas  Figure  5.3  attains  target  depth  in  approximately  5.5 
seconds.  In  Figures  5.5  and  5.6,  a  similar  comparison  is 
made.  Here,  however,  the  depth  change  is  considerably 
smaller.  Similarly,  the  response  time  difference  is  less 
noticeable . 

As  one  would  expect,  a  larger  actuator  in  the  dive  plane 
drives  a  quicker  AUV  response.  While  actuator  rate-of- 
change  limits  may  be  an  additional  limit  to  consider,  these 
results  assumed  that  such  a  limit  was  not  present. 

D.  EFFECT  OF  VEHICLE  SPEED 

Though  the  closed  loop  gains  were  designed  to  operate 
the  AUV  at  a  speed  of  2  FT/s,  other  speeds  were  tested  to 
determine  the  robustness  of  the  controller  so  obtained.  In 
Figures  5.7,  5.8,  and  5.9,  the  vehicle  speeds  of  two,  four, 
and  six  FT/s  respectively  were  evaluated  when  the  AUV  depth 
changed  from  six  to  four  volts.  In  Figure  5.7,  the  AUV 
smoothly  reaches  the  assigned  depth  without  overshoot  in 
approximately  five  seconds.  In  Figure  5.8,  target  depth  is 
attained  in  nearly  3.5  seconds  with  a  slight  overshoot 
before  leveling  out.  Finally,  in  Figure  5.9,  overshoot  is 
most  noticeable,  but  target  depth  is  accomplished  in  close 
to  three  seconds.  Based  on  these  observations,  it  can  be 
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Figure  5.4  Depth  Maneuver  from  1  to  5  Feet  with  Saturation 

at  10°  Dive  Plane  Anale.  Speed:  2  Ft/s  at  20  Sample  Rate 
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TIME(INCREMENTS  OF  .05  SEC) 
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TIME(INCREMENTS  OF  . 


noted  that  through  a  300%  change  in  the  speed  parameter,  the 
closed  loop  control  performance  remains  stable  with  only  a 
40%  change  in  response  time  and,  for  overshoot,  a  change  of 
less  than  15%. 
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VI .  BIAS  ESTIMATION  AND  RESULTS 


A.  GENERAL 

In  this  chapter,  the  final  stages  of  program  design  are 
implemented.  The  results  are  graphed  and  evaluated.  Here, 
the  fullstate  feedback  controller  in  Chapter  V  is  combined 
with  a  parameter  estimator  and  state  observer  system  as 
shown  in  Figure  6.1.  The  appendix  details  this  program 
code . 

The  parameter  estimator  determines  sensor  bias  of  the 
depth  pressure  cell  and  pitchrate  gyro.  The  three-state 
observer  determines  the  missing  state,  pitch  angle,  and 
filters  the  return  signal  of  the  remaining  two  states. 

with  the  inclusion  of  the  above  algorithms  (over  400 
lines  of  additional  code),  the  20  Hz  sample  rate  or  0.05 
seconds  per  sample,  had  to  be  reduced.  Significant  increase 
in  computation  time  prevented  a  sample  rate  much  higher  than 
5  Hz  or  0.2  seconds  per  sample.  Hence,  the  new  sample  rate 
became  5  Hz  and  the  poles  in  the  z-plane  were  changed 
accordingly.  The  remaining  figures  in  this  chapter  show  the 
results  of  the  new  code  at  work.  For  purposes  of 
evaluation,  vehicle  depth  change  remains  constant  while 
other  parameters  are  varied. 
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Diagram  of  the  Bias  Compensated  Model  Based  Controller 


Figure  6.1  Diagram  of  the  Bias  Compensated 
Model  Based  Controller 
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B.  EFFECT  OF  SPEED  CHANGE  ON  AUV  RESPONSE 

In  Figure  6.2  at  an  AUV  speed  of  2  ft/sec,  a  smooth  dive 
maneuver  from  1.0  volt  to  5.0  volts  yields  a  response  time 
of  13  seconds,  as  compared  to  six  seconds  in  Figure  5.4. 
The  decrease  in  sample  rate  has  moved  the  poles,  retarding 
AUV  response  time  by  more  than  a  factor  of  two. 

In  Figure  6.3,  the  vehicle  speed  is  doubled  to  four 
ft/sec  and  the  same  maneuver  is  invoked.  Here  the  depth 
response  curve  is  not  as  smooth  or  direct,  but  no  overshoot 
is  observed.  Since  the  poles  are  designed  for  a  slower 
speed,  the  vehicle  tries  to  settle  out  prematurely,  delaying 
the  depth  change  process.  This  maneuver  was  accomplished  in 
17  seconds. 

Results  of  this  vehicle  speed  increased  to  six  ft/sec 
are  shown  in  Figure  6.4.  Again,  the  curve  is  not  smooth, 
with  abrupt  settling  occurring  after  two-thirds  of  the  depth 
change  and  then  continuing  on  to  5.0  volts  depth.  The 
completion  time  for  this  maneuver  is  greater  than  20 
seconds . 

For  each  increase  in  speed,  maneuver  completion  time  is 
extended,  but  target  depth  overshoot,  however,  does  not 
occur. 

C.  AUV  RESPONSE  TO  A  BIAS  SIGNAL 

As  mentioned  in  preceding  chapters,  the  return  signals 
from  the  relatively  inexpensive  sensors  onboard  the  30  inch 
model  are  prone  to  include  sensor  bias.  In  the  subsequent 
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TIME(INCREMENTS 


figures,  the  results  of  vehicle  depth  with  sensor  bias  is 
shown.  A  0.1  volt  sensor  bias  in  Figure  6.5  results  in  a 
corresponding  0.1  volt  decrease  in  actual  depth  as  compared 
to  the  target  depth  of  five  volts.  Although  maneuver 
completion  time  is  not  influenced  (equivalent  to  Figure  6.2 
with  zero  bias) ,  the  target  depth  is  never  quite  reached. 
Similarly,  in  Figure  6.6,  a  larger  sensor  bias  of  0.2  volts 
results  in  a  vehicle  depth  falling  short  of  the  target  depth 
by  the  same  amount. 

By  slightly  adjusting  the  program  code,  the  estimated 
bias  can  be  removed  from  the  system  resulting  in  a  cleansed 
depth  response  signal,  shown  in  Figures  6.7  and  6.8. 
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VII .  SUMMARY  AND  CONCLUSION 

A .  SUMMARY 

This  thesis  presents  the  implementation  of  a  real  time 
autopilot  dive  plane  controller  for  the  NPS  AUV  testbed. 
The  controller  includes  a  state  observer  and  sensor  bias 
estimator.  The  approach  to  the  design  of  the  autopilot 
includes : 

-  Manipulation  of  vehicle  dive  dynamics  to  develop 
linearized  equations  of  motion, 

-  Implementation  of  an  Analog  Simulator  using  data  from 
open  loop  control.  This  affords  more  effective  and 
efficient  development  of  the  full  state  and  partial 
state  feedback  control  algorithms, 

-  Hardware/Software  interface  via  A/D  and  D/A  conversion 
boards , 

-  Program  development  using  Turbo  Pascal  version  3.0, 

-  Sample  rate  control  on  program  code  of  differing 
magnitudes, 

-  Incorporating  data  recovery  via  file  opening  techniques. 

B.  CONCLUSION 

This  study  developed  a  dive  plane  autopilot  for  the  NPS 
testbed  that  showed  signs  of  robustness.  Under  closed  loop 
control,  the  vehicle  stabilized  at  various  depths  under  wide 
ranges  of  speed  with  no  overshoot. 

As  program  code  increased  in  size,  throughput  increased 
reducing  sample  rate,  and  therefore,  reducing  autopilot 
response  time.  Future  research  of  the  steering  control 


autopilot  may  need  to  optimize  current  algorithms  to  prevent 
further  throughput  increase  and  to  optimize  robustness 
characteristics.  Parallel-tasking  processors  may  be  an 
option  to  large,  complex,  programming  code,  creating  a 
faster  throughput. 
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APPENDIX 


CONTROL  CODE  FOR  AUTOPILOT 


t  Sc  -  > 
program 


AuvAutoPllot  (  Input;,  output  ); 


(  TITLE 
AUTHOR 
APPLICATION 


Autonomous  Underwater  Vehicle  Auto  Pilot  Program. 
t,T  Jerry  J.  Relna  USN 

real-time  controller  with  a  Model  Based  Compensator  to 
estimate  pitch  angle,  and  a  parameter  estimator  to  determine 
sensor  bias 
8  Dec  1988 


Project  Description  ;  This  program  Implements  digital  control  of  the  NPS 
autonomous  underwater  vehicle  ( AUV )  In  the  vertical  or  dive  plane.lt  samples 
vehicle  sensor  Input  from  two  channels  :  depth  and  pltchrate.  The  Model 
Base  Compesator  determines  pitch  angle.  All  states  are  then  multiplied  by 
the  controller  gains  and  compared  to  the  target  depth.  An  appropriate  dive 
command  Is  determined  and  sent  back  to  the  vehicle  via  D/A  conversion  boards. 
A  deflection  on  the  dive  planes  result.  The  sample  rate  Is  5  Hz . 1 


{  GLOBAL  DECLARATIONS 


const 
,  - 


Screen  declarations 

=  5; 

=  2; 

■  75; 

=  24; 


type 

strlO  =  string  1101; 
str60  =  string  1601; 


(  Upper  left  corner  :  left  edge  ) 
f  UppCi  left  corner  :  upp«-r  edge  1 
t  Lower  right  corner  :  right  edge  ) 

{  Lower  right  corner  :  bottom  edge  ) 


hr,hr2,min,mln2, 

sec,sec2,hun,hun2 

seconds 

option,  controlmode, 
reply, reply2 


:  byte; 
:  real; 

:  char; 


INCLUDED  FILES  Declarations 


f  $  I  pcldefs.tp  1 
($1  pclerrs.pas  1 


(  PC  LAB  Trubo  Pascal  routines. 

1  PC  LAB  error  code  messages  file. 


( SIdrawbox2 .auv) 

t  Input  xl,yl,x2,y2  :  integer  to  specify  the  corner  limits  of  the  box. 
This  procedure  clears  screen  and  draws  a  rectangular  box  of  specified 
dimension  using  ASCII  double  line  characters. 

(SIclrbox2.auvl 
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I  Input  xl,yl,x2,y2  :  Integer  to  specify  the  corner  limits  of  the  box. 

This  procedure  uses  a  FAST  merits  of  clearing  a  box  of  specified  dimension. 
The  box  dimension  should  be  delcared  as  constants.  1 

t  $  I boxpr 1 n t  .  au v  1 

1  Input  the  prlntrow,  leftboxedge,  rlghtboxedge  :  Integer  and  printstring  : 
str60.  This  procedure  centerprlnts  the  string  In  the  box  at  the  prlntrov 


specified  without  overwriting  the  box  border.  ) 

ISIshowfast  .auvl 

t  Input  message  :  strFO,  column, row  :  Integer.  To  specify  *-hc  x,y  position 
on  the  screen  for  a  FAST  message  print.  ) 

l  $  1 keyh 1 1 .auvl 

I  This  1 3  a  boolean  function  which  returns  true  or  false  If  key  Is  pressed; 

It  also  returns  keycode  replies  VAR  reply,  reply2  :  char  .  ) 

( $  I tabxy .auvl 

1  Input  tabcol , tabrow  :  Integer;  like  gotoxy  ) 

1  $  I boxpause  .auvl 

1  Input  xpause , ypause  :  Integer  to  specify  where  "Press  any  key  to  continue" 
message  Is  to  be  printed.  1 

( Slge tkey . auv  ) 

1  Input  as  a  string  of  chars,  the  set  of  acceptable  replies;  le  'YyNn'.  This 
procedure  waits  until  one  of  the  acceptable  replies  has  been  entered.  ) 


{  *****************  MAIN  FROGRAMS  PROCEDURES  ****************************  ) 
(  *****************  U5ER  INTERFACE  MODULES  ****************************  } 

procedure  MainMenu  (  var  reply  :  char  ); 

(  This  procedure  presents  the  AUV  screen  and  solicits  an  option  to  Run 

the  AUV  from  the  StatusAndCommand  procedure  or  to  Quit.  ) 

beg  1  n 

repeat 
c 1 r  scr ; 

drawbox2(xl ,y1 , x2,y2) ; 

boxpr 1 nt ( yl +3, xl , x2, ' N  AVAL  POSTGRADUATE  SCHOO  L'l; 
boxprlntlyl *5,xl,x2, 'D  EPARTMENT  OF'); 

boxprlntlyl»6,xl,x2, 'M  ECHANICAL  ENGINEERIN  G'); 

boxprlnt(yl+8,xl,x2, 'AUTONOMOUS  UNDERWATER  VEHICLE'); 
boxprlntlyl HO, xl,x2, 'DIGITAL  AUTOPILOT  CONTROL  PROGRAM'); 

boxpr 1 nt ( y 1 i 1 5 , xl , x 2 , ' Do  You  want  to  RUN  this  program  ..'); 
boxprlntlyl >16,xl,x2,  'or  Do  You  want  to  QUIT  and  return  to  DOS  ?'); 
boxpr 1 nt ( y 1  a  20, xl, x2, ' >>>>  ENTER  Q  OR  R  <<<<'); 
getkey  ( 'QgRr ' , r epl y , repl y2  )  ; 

until  (  reply  In  ( ' Q ' , ' q  '  , ' R  ' , ' r  '  I  1  and  (reply2  =  chr(Ol); 
end ; 
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procedure  Status AndCommand  (  var  mode  :  char  ); 

!  This  procedure  begins  the  control  program  screen. 


) 


mode  2 


char  ; 


procedure  StatusAndCommandScreen; 

I  This  is  the  status  and  control  screen  and  solicits  a  user  Input  of  FI  to 
RUN  the  program  or  Q  to  Quit  and  exit  to  the  main  menu.  ) 


Status AndCommand Screen 


1 


begin  (  - 

clthox2  ( x 1 , yl , x2 , y2 ) ; 

boxpr inttyl »l,xl,x2, 'AUV  STATUS  /  COMMAND  AND  CONTROL  SCREEN'); 
boxprlnt(yl*2,xl,x2, =  -  =  =  =  =  =  =  -  =  =  =  “  =  =  =  =  =  =  =  =  -  =  -  =  -  =  =  =  =  =  =  =  ”  =  =  =  =  =  =  = 
boxpr int ( yl 47 , xl, x2, ' CHOOSE  YOUR  DESIRED  CONTROL  MODE  :'); 
boxpr  lilt  (yl  49,  xl,  x2,  '  ENTER  KEY  <<  FI  >>  TO  START  AUV  CONTROL’); 
bnxprlnt(yl4ll,xl,x2, 'ENTER  <<  Q  >>  TO  QUIT  AND  RETURN  TO  MAIN  MENU'); 
boxpr  Int(yl4l6,xl,x2,  'PRESS  EITHER  FI  OR  Q’); 


e  nd  : 


l 


StatusAndCommandScr een 


) 


CLOSED  LOOP  CONTROL  ROUTINES  ********************  ) 


procedure  Cl osedLoopCon t r o 1 ; 

I  This  module  comprises  the  closed  loop  control  scheme, 
label  5; 

LABEL  1; 
const 

maxdepth  =  33; 

mlndepth  =  0; 

update  1 ncrement  =  10; 


type 

try  =  arraytl..lO)  of  real; 
activecontrolmode  =  (  run,  reset,  exit  ); 
allovabledepthrange  =  m 1 ndepth ., maxdepth  ; 

auvattltude  =  (  climb,  maintain,  diving  ); 


d lgl tal lntegerarray  =  array  II. .31  of  Integer; 


) 


va  r 

filename istrlng (141; 
fllevaritext; 

auvdepth,  auvdepthvol ts , auvspeed , auvpl tch, auvpl tchvol tn , auvp  1  tchvoltsxx , 
a u vs peed volts,  auvp ltchr ate,  auvp ltchratevolts,estdepth, err, 
deptherrorvolts,  targetdepthvolts  , bl as  1, dive volts, tgtnev, tar getdepth, speed 
ea  1  ; 


adv 

j , status , t (me 
modcreply,modereply2 
act  1 vemode 

updatecounter ,  Initial 
depthrange 


digltallntegerarray; 

Integer; 

char  ; 

act l vecontrolmode ; 
Integer; 

allovabledepthrange; 


r 
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r 


a t  t 1 1  udn 
■1C  1 
nr  2 

ac  3 


auvatt  1  tude ; 
real  ; 
real  ; 
real; 


f tocedute  GetTar ge tDcpth 


(var  tgtdepth  :  real  ; 
var  tgtdepthvn 1 ts  :  real 


) ; 


(  TI1I5  procedure  solicits  the  target  AUV  operating  depth  and  converts  It  to 
an  At'V  equivalent  targetdepth  analog  voltage  and  passes  both  oE  these 
parameters.  ) 


tegln  1  -  Ge  tTa  r  ge  t  Dept  h  -  ) 

clrbox2  (xl,yl,x2,y2); 

boxpr  1  nt  (  yl  '  1  0,  xl  ,  x2,  '  ENTER  THE  AUV  TAPGET  OPERATING  PEFTH'); 
boxpr lnt(yl*ll,xl,x2, 'NOTE  :  THE  DEPTH  SHOULD  BE  IN  VOLTS  (0.0  -  10.0)'); 
repeat 
begin 

boxprlnt  ( y 1 H 3 , x 1 , x 2 , ' ENTER  THE  TARGET  OPERATING  DEPTH  '); 
got  ox  y  (xlt33,yltl5); 
read  (  tgtdepth  ); 
end ; 

until  tgtdepth  >=  0.0  ; 
tgtdepthvolts  :=  tgtdepth  ; 

end;  (  -  GetTargetDepth  -  1 

procedure  RunModeScreen; 

(  This  procedure  displays  the  Closed  Loop  Control  Screen  In  the  RUN  MODE.  1 

begin  (  - RunModeScreen  -  } 

clrbox2  ( xl , y 1 , x2 , y2  )  ; 

boxpr lnt ( yl *1, xl , x2,  *  A  UV  STATUS  /  CONTROL  SCREE  N') 

/ 

boxprlnt(y342,xl/x2,,=-  =  --r-.  =  =  =  =  =  ---  =  =  =  =  =  =  ?-  =  =  =  r:T  =  --  =  ------  =  =  =  =  =  -  =  =  =  =  =  =  =  -<). 

boxpr lnt ( ylM , xl, x2, ' STATUS  OF  A  U  V  OPERATING  PARAMETERS  :’); 


write 

( tabxy 

(xlt5,yl46), 'AUV 

DEPTH 

(volts) 

: 

write 

( tabxy 

(xl45,yl47 ) , ' AUV 

PITCH 

t  volts  ) 

!  ’  >; 

write 

(tabxy 

( x 1 4  5, y 1 *8), ' AUV 

P I TCHR ATE 

(volts) 

S  ’ ); 

write 

write 

(  tabxy 
( tabxy 

(xl45,yl*9>, ' AUVDI VEPLANE 
(xl 45, yl*10) , 'BIAS  = 

(volts) 

;  ' ); 

boxpr lnt ( yl *  1 1, xl, x2,  ' A  U  V  CONTROL  STATUS 

write  (tabxy  ( x] *  5 , yl 4 1 3 ) , ' CURRENT  TARGET  DEPTH  :  '); 
write  (tabxy  ( x 1 4 5 , y 1 4 1 4 ) , • CURRENT  MODE  :  ’); 
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write  ( tabxy  (  xl  i  5, y 1 4 15 ) ,  ’ CURRENT  MANEUVER  : 

boxpt i n  t ( y 1 < 18,xl,x2, 

’ ERROR  KEY  FI  ..  TO  ENTER  NEW  TARGET  DEFTH.  ’); 

boxpr  ltit(  y  1  >  19,  x  1 ,  x2 , 

•TRESS  KEY  F2  ..  in  STOP  ACTIVE  CONTROL  ANN  PESET . ' ) ; 
boxpr  1 nl ( yl *  2h,xl, x2, 

'  PRESS  KEY  F  3  ..  TO  EXIT  ACTIVE  CONTROL.  '); 

end;  (  -  RunModeScr  een -  1 

procedure  UpdateRunHodeScreen  (updatedepth,updatepitch,updatepitchrate 
, updated  1 vevolt  :  real; 

updatetnrgetdepth  :  real; 
updatemode:  act  1 vecont r o 1  mode  ; 
tipda  tea  1 1  i  t  tide  :  auvattltude; 
var  blasl:  real); 

(  This  module  updates  the  Closed  loop  Control  Run  Mode  Screen  with  updated 

display  parameters.  Updates  occur  In  Intervals  specified  by  upda te  1  ncr emen t 
interval  declared  In  C 1 osedLoopCon t r o 1  procedure.  } 

begin  1  -  Upda  teRunMode  Scr  een  -  } 

l  UPDATES  STATUS  OF  A  U  V  OPERATING  PARAMETERS  ) 

vrlteln  (tabxy  ( xl ♦ 37 , yl 4 6 ) , upda tedcpth : 6  :  3  ) ; 
wr 1 1 e 1 n  (tabxy  ( xl *  37,yl 4  7 ) , updatepl tch : 6 : 3  ) ; 
wrlteln  (tabxy  ( xl *  37 , y 14 8 ) , upda tep 1 tchrate  :  6  :  3  )  ; 
wrlteln  (tabxy  ( xl  4  37, yl *  9 ) , updated ) vevol t : 6 : 3 ) ; 
wrlteln  (tabxy  ( xl 4 37  , y 1 4 10 ) , b  las l :  8  :  6  ) ; 

(  UrDATFS  THE  A  U  V  CONTROL  STATUS  I 

write  (tabxy  ( x 1  +  30, y 1 4 l 3 ) , upda t e tar ge tdepth  :  G  :  2  ) ; 
case  updatemode  of 

run  :  wrlteln  (tabxy  ( xl 4 30 , y 1 +1 4 ) , ' RUN  '); 
reset:  wrlteln  (tabxy  ( xl +  30 , y 1 ♦ 1 4 ) , ' RESET  '  ) ; 
exit  :  wrlteln  (tabxy  (xl430,yl4l4), 'EXIT  '); 
end  ; 

case  updateatt  1  tude  of 

maintain  :  wrlteln  (tabxy  (xlt30,yl+15), 'MAINTAINING  DFFTH  '); 

cl  1  mb  :  wrlteln  (tabxy  ( xl 4 30, yl 4 15 ) , ' CLIMBING  TO  TARGET  DEPTH'); 

diving  :  wrlteln  (tabxy  ( xl 4  30, yl ♦ 1 5 ) , ' Dl VI NG  TO  TARGET  DEPTH  '  )  ; 

end ; 

end;  ( - Upda  t  eR  unMode  Scr  ee  n - ) 

procedure  Ge t D 1 g 1 ta lSensor yDa ta  (  var  depthana 1 og vol t s , p 1 tchana log vo 1 t s , 

pitchrateanalogvolts  :real)  ; 

(  Tills  procedure  uses  PCLAB  routines  to  sample  selected  Input  telemetry 
channels  from  the  AUV  and  digitizes  these  Inputs  and  multiplies  them 
by  the  specified  gains. 

DT  2801  -A  /  DT  707  Board  set  up:  channel  1  -  AUV  depth  Input 

ctiannel  2  -  AUV  pitch  Input 

channel  3  -  AUV  pltchrate  Input  ) 
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cons  t 


(  These  are  AUV  to  DT  2801 -A  /  DT  7 07  hook  up  board  channel  configurations, 

conversion  and  computational  arguments.  1 

d ept hcha line  1  =  1;  I  AUV  output  to  DT-707  Input  channel  assignment  1 

depthpfs  =  +10.0;  t  Teak  depth  signal  value  ) 

depthmfs  =  -10.0;  (  Minimum  depth  signal  value  1 

pitchchannel  =  2;  I  AUV  output  to  DT-707  Input  channel  assignment  1 

spdpfs  =  +10.0;  l  Teak  pitch  signal  value  1 

spdmfs  =  -10.0;  (  Minimum  speed  signal  value  1 

p 1 tchr a t echanne 1  -  3;  (  AUV  output,  to  DT-707  Input  channel  assignment  1 

pttchratepfs  =  +10.0;  {  Peak  pltchrate  signal  value  1 

pltchratemfs  =  -10.0;  1  Minimum  pltchrate  signal  value  ) 

noc  =  4096;  l  Number  of  Codes;  conversion  resolution. 

The  DT  2801-A  performs  a  12  bit  conversion. 

NOC  =  (2  *  conversion  bits),  le  4096  ) 


t  SetUpAdc  and  ADConTrigger  PCL  function  arguments 

:  p  6-8  PCL  documentation  1 


boardnum 

=  i; 

numa  2dchan 

-  3; 

t  lmlngsource 

-  2; 

{  --  Sets  a  external  tr lgger ,  Internal  clock  1 

adcga In  1 

=  l; 

adcga 1 n2 

=  2; 

adcgalr.4 

=  4; 

1  Sets  the  A/D  gain;  1,2, 4,8  are  options 

adcga 1 n8 

=  8; 

s  tar  tchanne 1 

=  i; 

endchannel 

=  3; 

var 

p 1 tchadv, 
depthadv, 
p! tchrateadv, 

slgnaladv,  (  Signal  analog  data  value  1 

counter , status , 

chanuir.,1,3  :  Integer; 

begin  (  -  procedure  Ge t D 1 g 1 ta 1 Te 1 eme t r y  -  ) 


(  Set  up  the  DT  2801-A  board  to  take  data.  1 

status  SelectBoard  (boardnum); 

(  Set  up  the  DT  2801-A  board  to  take  data  from  3  Input  channels;  Data  sampling 
Is  Initiated  by  the  ADConTrigger  single  channel  sample  of  the  depth  channel 
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and  then  single  A DC  \alue  samples  of  the  speed  and  pltrhrate  follow. 

The  Trigger  Is  connected  to  the  DT  707  board  at  terminal  49  from  a  signal 
generating  source.  ) 

status  :  =  ADConTrlgger  (  depthchanne 1 ,  odcqalnl,  depthadv  ); 
status  APCValuc  (  pi  tctichanrie  1 ,  adcgalni,  pi  tchodv  ); 
status  ADCValue  (  p 1 tchra techanne 1 ,  adcgalni,  pltchrateadv  ); 


I  ronvert  the  digitized  Analog  Data  Values  for  pitch,  depth,  pltchrate  to 
analog  voltage  values.  The  algorithm  for  this  conversion  is  found  In 
Appendix  D  of  the  PCLAB  documentation.  ) 


depthanalogvolts  :=  (  depthadv  *  ( depthpf s -dept hmf s ) /noc 
p  1  tchana 1 ogvo  1 1 s  :=  (  pltchadv  *  ( spdpf s -spdmf s ) /noc  ) 


+  depthmfs; 
+  spdmfs; 


pltchrateanalogvolts  :  =  (  pltchrateadv  * 

(pltchratepfs  -  pltchratemf s ) /noc  ) 

+  pltchratemfs; 

(  status  :=  Terminate;  1  1  -  PCL  routine,  closing  the  DT  2801-A  1 

end;  (  -  procedure  Ge tD 1 g 1 ta ITe leme tr y  -  1 

procedure  ATT  I TU0E_  (  tdepthvolts,  adepthvolts  :  real; 

var  attitude  :  auvattitude  ); 


const 

de pt hcont r ol t o ler ence  =  0.1; 

mode  1 ga In  =1.0; 

var 

voltsd  1  f ference  :  real; 


1  This  simulates  a  model  referenced  1 
(  gain  parameter  ) 


begin  (  - Errorvolts  - 

vo 1 tsd l f f e r ence  :=  tdepthvolts  -  adepthvolts; 


) 


Control  voltage  filter 


1  These  conditions  check  If  depth  Is  within  tolerence.  1 

If  (  vol tsd 1 f ference  >  0)  and 

(  abs ( vol tsd 1 f ference )  <=  de pthcontr ol tolerence  )  then 

beg  1  n 

attitude  :=  maintain; 
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eivj 

else  If  (  vo  1  tsd 1 ££ e r ence  <  0)  and 

(  abs ( vo] tsd 1 f f erence )  <-  depthcon t r o 1 t o 1 e r ence  )  then 

begin 

attitude  :=  maintain; 

end 

1  This  condition  checks  If  actual  depth  is  less  than  target  ♦  tolerence.) 

else  If  (  vol tsd 1 f f erence  >  0)  and 

(  abs ( vol tsd I f f e r ence )  >  depthcont r ol t o ler ence  )  then 

beg  1  n 

attitude  :=  diving; 

e  nd 

(  This  last  condition  checks  to  see  if  the  actual  depth  is  more  than  target  + 
tolerence .  ) 

else  if  (  vol tsd 1 f f erence  <  0)  and 

(  abs ( vol tsd 1 f ference )  >  depthcontroltolerence  )  then 

beg  I  n 

attitude  :=  climb; 
end ; 


end;  (  - Errorvolts 


) 


procedure  ESTlvar  up, yg, z : real ; var  vha t , p 1 tch : r ea 1 ; var  In  It  lal :  Integer ; 

var  b las , al , a  2 ,  a  3  :  real); 

(This  procedure  uses  a  Model  Based  Compensator  to  determine  pitch  angle. 
A  parameter  estimation  using  RLS  fit  determines  sensor  bias.) 

label  3; 

type 

try  =  array(1..10!  of  real; 
trytry  =  a r ra y t 1 . . 10 , 1 .  .  10  I  of  real; 
try40  =  arrayll  .  .  40, 1 .  .  40  1  of  real; 
tryt40  =  arraytl..401  of  real; 

const 

ns  =  1  ; 


var 

beta,g,khat,xdata,g2,phld,phiq,fc,phl2,khat2,xhat2,xhat, 

g3 , h3 , khat 3 , ph 1 , pha t , xu , xy , xz, xs , xnew  :  try; 

spt , f , spt 2 , f 3, Spt 3  :  trytry; 

nx , np, ny , nl , nf , n2 , nsnf , t lme , 1 , j  :  Integer; 

zd, zf , zf  2, uf , y f , f s ,af , y, yhat , ud, u, yd, pO, sr  3, sr  2, sr , sq3, sq, sq2, 
zdd,sg2,slgmax,gblas  ;  real; 
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procedure  1 n 1 1 1  a  1 1  ze  Ju  r ays ; 
beg  1  n 

for  1  :=  1  to  10  do 
begin 

beta  II ) : =0 . 0; 
that! 1  1  :  =  0  .  0  ; 
xhatll)  0.0; 
phllll  :=  0.0; 
x  u  1 1  )  •*  =  0 . 0 ; 
xy(l) : =0 .0; 
x  z  J  1  ]  :  -  0  .  0  ; 
xnevl 1  1  : =0 .0; 

end  ; 

np  : ■=  2 *ns ; 
nx  :=  np  4  1; 
ny  :=  1; 

y '•  -  0.0; 

u  £  :  =  0.0; 
ud : -0 . 0 ; 
yd : =  0 . 0 ; 
y£  :=  0.0; 
zd  :=  0.0; 
z  f  : =  0.0; 
time  : =  0; 

(slgmax  =  variance  of  the  bias) 

slgmax  :=  0.1; 

sq  :=  slgmax; 

sq2:  =  slgmax; 

sq3  :=  slgmax; 

sr  :=  1.0; 

sr2  : =  1.0; 

sr  3  : *  1.0; 

p0  :=  1 . 0e6  ; 

for  l : =  1  to  nx  do 
begl  n 

for  j:=  1  to  nx  do 
begin 

s  pt ( 1 , j )  : =  0.0; 
s  pt  2 (  l , j  1  : -  0.0; 
f(l,3)  :  =  0.0; 
f3ll,31:=0.0; 

If  1  -  j  then 
begin 

spt (1,3):=  p0; 
spt2 l l,j): =p0 ; 
spt  3  I  1,3  1 : =p0; 
f ( l , 3 1 : =  1.0; 
end; 

end ; 

g! 1 ) :=  0.0; 
g2(l 1 :=  0.0; 


end ; 
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g ( n  x 1 : =  1.0; 
g  2 ( 2  ]  :  -  1.0; 

(  fell]  :=  2  .9  27  2F.-2; 

£c l 2  )  —  -6.9013E-1; 
f  c  1  3  1  :  =  1.3633E-1; 
feM  !  :  =  -7 . 5566E-2; 

£c(5!  :  =  0.0; 

£ct6  1  : =  9 . 8851E- 2; 

£c 1 7  ]  :  =  3  .  9545E-1 ; 

£c 1 8  1  —  5.9317E-1; 

£c 1 9  I  :  =  3.9545E-1; 
fcl  10 1 :  =  9  .  8861E-2; ) 

£c(l I : =0 . 80000 ; 
f  c  (  2  1  —  0.20000; 
nf  :  =  1; 

£s  :=  5.0; 

£  3 { 1, 1 J  :  =  0. 6977;  {discretized  A-matrlx) 

£312,11—0.10  80; 

£312,21 :=1.0; 

£313,11 :  =-0 . 9178; 

£313,21 : =-0 . 20; 

£313,31 : = 1 . 0 ; 

g 3 1 1 1 :=0. 4251;  {discretized  B-matrlx) 

g  3  t 2 1 : =0.04  51; 
g 3  I  3  1  :=-0 . 0031; 

{  k  ha 1 3 {  1  1 :=-0.0073;khat3t 2) :=-0.1046;khat3( 3  1  —0. 4250;  1 

(gains  for  5Hz . , poles@ 1 0 . 75  0.76  0.77]  kob7  1 
khat3l 11 :=0.0;khat3121 : =-0 . 4 029 ; khat 3 1 3 1 : =0.5677; 

{ k ob8  @  ob8poles  =  { . 7  .71  .72)  at  l£t/s) 
bias : =0 . 0; 

end ; 


procedure  1 nne r pr od ( var  y9:  real;var  phi  9 , x9 : tr y ; var  nx9 : 1 ntege r ) ; 


var 

j:  Integer; 
beg  1  n 
y9 : =  0.0; 

for  j  :=  1  to  nx9  DO 
begin 

y9  :=  ph 19131  *  x9Ijl+  y9; 

end ; 
end; 

procedure  updatefvar  phl8:  try;var  u8,y8:  real;  ns:lnteger); 


var 

n2 : Integer ; 

1 :  Integer ; 
phlsave:  try; 
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begin 


n2:=ns*2+l; 
for  1  : =1  to  n2  do 
begin 

phlsavel 1 1 :=phl8t 1 1 ; 
end ; 


1  :  -  ns ; 
while  1  >=  2  do 
begin 

ph 1 8  I  1  )  :  -  phlsavel 1  -  11; 
phlBtns  +  11  :=  phisave 1 ns+ 1 -1 ) ; 
1  :  =  1  -  1  ; 
end ; 

phi  8 1 1 1 : =  y 8 ; 
phl8tns  1  11  :=  u8; 

end ; 


procedure  fllterlvar  u4 , y 4 : r ea 1 ; var  x 4 , £c4 : tr y ; var  nf 4  :  Integer ) ; 
var 

n£42  :  Integer; 
savex4 : try ; 

begin 

update(x4,u4,y4,nf4); 
n f 4 2 : =  2  *  n£4; 
lnnerprod(y4,x4,fc4,nf42); 

t wr lteln ( ' Ins lde  filter  after  Inner , y 4 , x4  =■  ' , y 4 ,  x 4  1 1 1 ,  x 4  (  2 1 ,  x 4  1 

3  1); 

1 

end; 


procedure  newestlvar  nx7:  lnteger;var  xhat7 , khat7 ,  h7  :  try; 

var  y7:real); 

var 

delta:  real; 
l , j :  Integer  ; 
xsave : try ; 

begin 

for  1 : =1  to  nx7  do 
begin 

xsave II): =xhat7  111; 
end ; 

delta  :=  y7; 

for  ) : =  1  to  nx7  do 
beg  1  n 

delta:=  delta  -  h7tjl  *  xhat7|j); 
end ; 

for  1 : =  1  to  nx7  do 
begin 

xhat7[ll  :=  xsavell)  +  khat7lll  *  delta; 
end  ; 

end ; 
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procedure  hholder(var  nr6,nc6:  lnteger;var  ralG:  try40); 
va  r 

l,col,l,j,k:  Integer; 
temp,slgraa,sum  :  real; 
d6:  tryt40;  1 ar r a y ! 1  .  .  4 0  1  of  real;) 
c6 , h6 : t r  y 40 ; (  ar rayt  1  . . 40, 1  .  .  40  )  of  real;  ) 


begin 


begin 


for  col:  =  1  to  nc6  do 
sum  : =  0.0; 

for  j  :=  col  to  nr6  do 
begin 

sum:  =  sura  t  ml  6  [  j  ,  col  )  *ml  6  [  j  ,  col  )  ; 


end ; 

slgma:=  sqrt(sum); 
for  j : =  col  to  nr  6  do 
begin 

<36  (  j  1  :  =  0.0; 
end ; 

d6 (col  1 : =  s Iqma ; 
sum  : =  0.0; 
for  j:=  col  to  nr6  do 
beg  1  n 

sum  :=  sum  +  sqt (ml6 1 j , col  1 -d6 I j 1 ) ; 
end; 

for  j :  =  col  to  nr6  do 
begin 

for  k:=  col  to  nr6  do 
begin 

begin 

If  abstsom)  <  0.00001 
then  terap:=  0.0 

else  temp  :=  -2*(ml6lJ,col)-d6tjn*(iiil6f)c,col]- 

d6 ( k  1  ) /sum; 

end ; 

If  j  =  k  then 
begin 

temp: =  1.0  +  temp; 
end ; 

h6 [ J , k  J : =  temp; 
end; 

end; 

for  j  :  =  col  to  nr6  do 
begin 

for  k:=  col  to  nc6  do 
beg  1  n 

terap:=  0.0; 

for  1:=  col  to  nr6  do 

begin 

temp:-  temp  +  h6( J, 1 )*ml6i l,k  ); 

end ; 
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end ; 


c6 ( j , k ) : *  temp; 


end ; 

1  m!6 : =  c6; l 


for  i  : =col  to  nr  6  do 
begin 

for  j ; =col  to  nc6  do 
begin 

ml6 1  1 ,  j  1  : =c6 (  1 , j  ) ; 

end ; 

end; 

end ; 

for  1 : =  2  to  nr  6  do 
beg  1  n 

for  j  :  =  1  to  1-1  do 
begin 

ml6 [ 1 ,  j  )  :  =  0.0; 

end ; 

end ; 

end ; 

procedure  kgalntvar  nx5:  integer;var  spt5,£5:  trytry; 

var  g5, h5 : try ; var  sr 5 , sq5  :  r ea 1 ; var  khat5:  try); 

var 

ny ,  1 , j , nu, k , nytot, nx5nu :  Integer; 
ml5:  try40; 
sptt  ;  trytry; 
begin 

n>15tl,ll  :=  sr  5; 
ny : *  1  ; 
n  u  :  =  1 ; 

for  V : =  1  to  nx5  do 
begin 

for  j : =  1  to  nx5  do 
beg  In 

ml5lnyf 1 ,ny  +  j ) : =  s  pt  513,1); 

end ; 

end ; 

for  1  :  =  1  to  nx5  do 
begin 

ml51 ny+ 1,1)!=  0.0; 
for  k : =  1  to  nx5  do 
begin 

ml5(ny+l,l):=  ml 5 1 ny+1 , 1 1 +spt5 1 k, 1 1 *h5 1  k ) ; 

end ; 

end; 

for  j:=  nyfl  to  nytnx5  do 
beg  In 

m 1 5 [ 1 , j  J : =  0.0; 

end ; 

nytot:=  nytnx5; 

{  wrlteln  ('enter  hholder  first  time');  1 


68 


<  vr  i  telnt  ’  n>  1  5  '  ,ml  51  1,  1  1  ,ml5t  1, 2  ]  ,mlS(  1,  3  ])  ;  ) 
hholder(nytot,nytot,ml5)  ; 

(  vr  1  te  1  n  (  1  ml  5a f ter  hholdet  frst  ' , ml  5  I  1 , 1 ] , ml  5 1 1 , 2 ) , ml  5 1 1 , 3 1 ) ; ) 

If  a hs { ml  5 [ 1 , 1  1  )  <=  0.000000.1  th«-n  vrltclnf'R  is  singular'); 
for  1  :  -  1  to  nx5  do 
begin 

k ha  1 5 [ 1  )  : =  ml  5 1 1 , 1 U 1 /ml  5 1 1 , 1 1 ; 

(  vrltelnf ' kha 1 5 ' , k ha 1 5  til , '  ’,1);) 

for  j  :  =  1  to  nx5  do 
beg  1  n 

spttij,ll:=  ml  5 ! ny ♦ 1 , ny ♦ j 1 ; 

end  ; 

end  ; 

for  1 : =  1  to  n  x  5  do 
begin 

for  J  :  =  1  to  nx5  do 
begin 

m 1 5 1 1 , j 1 : =  0.0; 

for  k : =  1  to  nx5  do 

begin 

ml5{ 1 , J  )  : -  m 1 5 [ 1 , J)+sptt[k, 1 J*f5[j,k); 

end ; 

end ; 

end  ; 

for  j : =  1  to  nx5  do 
begin 

ml5l nx5+l , j ) : =  sq5*g5tj); 

end ; 

nx5nu  :=  nxS+nu; 

t  vt 1 te In (' enter  hholder  second  time');  ) 
hholder(nx5nu,nx5,ml5); 
for  1 ; *  1  to  nx5  do 
begin 

for  j  :  =  1  to  nx5  do 
beg  1  n 

spt5 1 j , 1 1 ; =  ml  5 ( 1 , j ) ; 

end; 

end ; 

end ; 


begin  C *»****beglnnlng  of  e s t lmat e r ****** } 

(He lteln( ’time*' , time, ' Initial*' , Initial);) 

If  Initial  =  1 
then 

begin 

lnltlallzeArrays; 
goto  3; 

end ; 
ud : =uf ; 

filter  (up, u£, xu,  fc,n.c); 
yd  :=  y£; 


filter! yq,y£,xy,£c,nf); 

zdd  :  =  zd ;  i 

zd  : =  zf; 

£  liter! z,zf,xz,  £  c  ,  n  £  )  ; 
update(phl,ud,yd,ns); 
ph  1  I  nx  )  :  - 1 . 0  ; 
nsn£  : =  ns  +  n£; 

1 1  m  e  :  =  1 1  m  e  +  1 ; 

If  time  <-  nsn£  then  goto  3; 

!  *****  estimate  bias  *****  } 

kga  1  n ( nx , spt , f , g, phi , sr , sq, khat  1 ; 

(  wrltet'phl  and  yf  Into  ne wes t 1 , ph 1  I  1 ! , ph 1  I  2 1 , ph 1 1 3 ) , y £ ) ; 1 
(  wr 1 1  e ( ’khat ’ ,khat 111 ,khat I  2 1 ,khatl 3 ) ) ;  ) 

newest! nx,xhat,k  hat, ph 1 ,  y £ ) ; 

(  wr 1 te 1 n ( ' par as  -  '  , 

xhat ( 1 ) , xhat I  2 1 / xhat I  3 1 , xhat  M 1 , xha 1 1  5  ! )  ;) 

(***  compute  bias  estimate  ***( 

1 : =  ns ; 

while  1  >=  2  do 
begin 

beta! 11:=  betatl  -  11; 

1:=  1  -  1  ; 

end ;  » 

beta ( 1 1 : =  bias ; 
bias  :  =  xhat [ nx  1 ; 

I  Wr 1 te In ( ' blas= ' , bias  ) ;  1 

for  J:=  1  to  ns  do  i 

begin 

blas:=  bias  +  xhat ( j 1 *beta ! j  1 ; 

end ; 

(  al :  =xhat 1 1 1 ;a2 : =xhat t  2 l;a3:=xhatI31;J 


(*********  estimate  ********* 
ph  1 2 ( 1 1  :=  y £ ; 

ph  1 2 ( 2  )  :=  1.0; 

z£2  :=  (z£  -  2.0*zd  £  zdd ) * sqr  !  £s  ) ; 
n  2  ;=  2; 

kgaln(n2, spt2, £, g2, phi  2, ar  2,sq2, khat 2 ) ; 

newest(n2,xhat2,khat2,phl2,z£2); 

If  abs ( xha 1 2 (11)  >  0.00000001 
then  gbias:=  xhat2 ! 2 I/xhat2 1 1 1 ; 
vhat: =  xhat2lll; 

***  estimate  pitch  angle  *** 

£3(1,1):=  1.0; 

£3(1,21:=  - 1/ £s ; 
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f  3  I  2 , 1  ]  :  -  0.0; 
f  3  I  2 , 2  )  :=  1.0; 

<33111  :  =  1.0; 
g  3 1 2  1  :  =  1.0; 
h  3 ( 1  1  : =  1.0; 
h 3 [ 2  1  : -  0.0; 

kgaln(n2,spt3,f3,g3,h3,sr3/sq3,khat3);  ) 

(observer  for  plytcli  a  jli,  10/2/8  8  ) 
begin 

for  1  :  =  1  to  3  do 
begin 

xne  v  ( 1  1  :  -  0 . 0 ; 
for  j  :  =  1  to  3  do 

xnev[l):=xnevtl)+f3tl,j)*xs(jl4g3tl)*up/10.0+khat3(l)*(z-xst3]4yq-xsll 
end  ; 

end;pltch:=xncv(21; 
xs 1 1 1 : =  xneu  11); 
xs  (  2  1  : =xnev 1 2  1  ; 
xs  I  3  )  :  =xrie  v  [  3  ) ; 

al:=xs(l);a2;=xs(21;a3: -xs  t  3  1 ; 


3 : end ; 


procedure  GenerateDl veplaneCommand  (var  auvdept h , auvp 1 tch , auvpl tchr a te , 
auvdepthcom, delta :  realjvar  k:lnteger  ); 

t  This  procedure  takes  digitized  voltage  values  of  depth , pi tch, pi tchr ate , 
and  target  depth  In  volys  and  sends  new  commands  for  control.} 

const 

I  DT  2801-A  DIGITAL  TO  ANALOG  Conversion  declarations  ) 

d2achannel0  =  0; 
pfs  -  10.0; 
mfs  =  -  10.0; 
noc  -  4096; 

T=0 . 2 ; amp=0 . 05; TO-2 . 0 ;  (these  parameters  Include  sin  excitation) 

(  gl=0.3066;g2=0.8604;g3--0.2493;) 

(these  gains  are  for  20  Hz.<?  poles=1.92  .925  .926)) 

(  gl=-0.2286;g2=0.1944;g3=-0.0118; 

these  gains  are  for  5Hz.  and  poles  5  (0.92  .925  .926)) 

gl  =  0. 2060 ;g2  =  0.6573;g3  =  -0. 1605; (for  5Hz.  at  10. .85  0.860  0.870)) 


var 

d 1 gl ta Ida ta va lueO ,  status  :  Integer; 

e 1 , e  2 , e  3  :real; 

function  ConvertAnalog2Dlgltal  (  analogvalue  :  real  ):  Integer; 
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t  This  function  converts  analog  signal  volts  to  an  equivalent 
digital  value.  See  App  D  of  PCLAB  book.  ) 

va  r 

temp  :  r  ea  1 ; 

begin 

temp  (  analogvalue  -  mfs  )  *  (  (noc  -  10)  /  (pfs  -  mfs  )  ); 

conver tana  1 og2d 1 g  1  ta  1  :=  round  (  temp  ); 

e  nd  ; 

begin  (  -  Gene r a t eD 1 veP laneCommand  -  ) 

e] : = (auvdepthcom-auvdepth ) ; 
e  2 :  =  ( auvp Itch)  ; 
el: =(auvpltchrate) ; 

If  abs ( e  3  )  <  =  0.000001  then  e3:=0.0; 

If  abslelX'0. 000005  then  el:=0.0; 

If  abs ( e 2 )  <  =  0.000005  then  e2:=0.0; 

delta  :  =g  3  *e  3  +■  gl*(-el)  *g2*(-e2); 

1  f (abs (delta ) >0 . 1 )  then 
beg  1  n 

delta  :=0.1*abs (delta) /delta; 
end  ; 

delta  :=  10*delta; 

dlgltaldatavalueO  :=  conver tana  1 og2d  1  g 1 ta 1  (  delta  ); 
status  :  =  dacvalue  (  d2achannel0,  dlgltaldatavalueO  ); 

(  status  :=  terminate;  ) 

end;  (  -  GenerateDlvePlaneCommand  -  I 

procedure  Hovellvar  r0,u,r  :real); 
const 

coun tma  x  =  A  00 ; 
count  2  =  200 ; 

(type 

activecontrolmode  =  ( run, reset , ex  1 1 ) ; 
auvattltude  =  ( c 1 lmb, ma 1 nta 1 n, d 1 v l ng ) ;  ) 


var 

count  : Integer  ; 
dr,xl,x2,x3  :real  ; 
actlvemode  : ac t 1 veCONTROLmode ; 
attitude  :auvattltude; 

begin 

actlvemode:=  reset; 
att 1 tude : =cl  lmb; 
count: =0  ; 


72 


vh 1 1 e ( count <countma x )  do 

begin 

d  r  :  =  0  .  r  ’  0  ; 

GctDlgl  dISensor yPata(xl,x?,x3); 
lf(count>count2)then  dr: =0.0; 
r :=r0>dr*count; 

! 1 f ( count >count 2 ) then  dr:=0.0;l 

GenerateClveplaneCommand  (xl,x2,x3,r,u, count)  ; 
count  :  =count  <-1 ; 

(  Upda teRunModeScr een ( x 3 , x 2 , xl , r , act  1 vemode , a 1 1 1 1 ude  )  ) 

end ; 

e  nd ; 


procedure  I n  1 1 1  a  1 1 zeFar ame ter s  ; 

t  This  procedure  Initializes  all  declared  control  and  display  parameters  to 
zero.  ) 

begin  t - procedure  1  n 1 1 la  1 1 zePar ame ter s ) 

auvpltchvolts  :=0.0  ; 
auvdepthvolts  :=  0.0; 
auvspeed vol ts  :=  0.0; 
auvpltcbratevolts  :=  0.0; 
auvdepth  :=  0.0; 
auvspeed  :=  0.0; 
auvpltcbrate  :=  0.0; 
estdeptb : =0 . 0; 
er  r : =0 . 0; 

end;  1 - procedure  I  n 1 1 la  1 lzeParameter a -  ) 


beg  1  n 


Act  1 veCont r  o 1 


1 


lnltlallzeparameters; 

Initial  :=  1;  11 n 1 1 la  1 1 ze Ar r ays  In  EST  procedure) 

5  : 

time  :=  1;  (Initializes  beginning  of  data  file) 

clrscr ; 

vr 1 te 1 n ( ' DATA  FILE  NAME?  lie  f082201.dat)'); 

readlnl f llename  ) ; 

ass  1 gn ( f 1 levar, f 1 lename ) ; 

revr 1 te ( f l levar  ) ; 

clrscr; 

activemode  :=  run; 

repeat  I  -  Repeat  until  activemode  =  exit  -  ) 


(  ClosedLoopControlScreen;  ) 

GetTar getDepth  (  targetdepth,  tar ge tdepthvol ts  ); 
RunModeScreen ; 
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1  : 

whll"  (  not  keyhlt  (  modcreply,  modereply2)!  do 
bcq  1  tl 

upda teco  inter  :  =  0; 

vtille  (  updatecounte  r  <  upda te 1  net ement  )  do 
begin 

GetDlgltalSensoryData  (auvdepthvolts,auvpltchvoltsxx,auvpltchratevolts); 


Gene  ra  teD  1  veplaneComma  nd  (  ac3,ar2,at:l, 

targeldcpth  volts,  dlv«»volts, time  ); 

EST  (dlvevolts,auvpl tchratevolts,auvdepthvolts, 

speed, auvpltch volts.  Initial,!' Iasi, acl,ac2,ac3)  ; 
wrlteln(£llevar,tlme:5,auvdepthvolts:12:6,auvpltchvoltsxx:12:6, 
auvpltch ratevolts:12:6,auvpltchvolts:8:3,dlvevolts:12:6, 
bias  1:12:6, ac 1:12:6, ac2;12:6,ac 3:12:6); 


ATTITUDE_  ttargetdepthvolts,auvdepthvolts, attitude); 
Initial  :  -  0; 

upda tecounter  :=  updatecounter  +  1; 
time  : =  time  ♦  1 ; 

end;  t  while  updatecounter  <  upda te 1 ncr ement  1 


Upda t ePunModeScr een  (  auvdopthvel ts ,  a u vp 1 t ch vo 1 t s xx , 

auvpltchratevolts,dlvevolts,targetdepth,  activemode,  attltude,blasl); 

end;  (  while  not  KcyHlt  1 

It  ( ord ( moder epl y  )  =  27)  and  ( ord ( modereply 2 )  =  59)  then  l  Keylllt=Fl  ) 
begin 

close ( f llevar ) ; 

goto  5; 

end 

else  If  ( ord ( moder epl y )  =  27)  and  ( or d ( moder eply2 )  =  60)  then 
beg  In 

activemode  :  =  reset; 

Movel(targetdepthvolts,dlvevolts,tgtnew); 
t ar ge tdepthvol ts : = tg tnew  ; 

GOTO  1 
end 

else  If  ( ord (moder eply )  =  27)  and  ( ord (modereply2 )  =61)  then 
begin 

close ( £  1 levar  ) ; 
activemode  :=  exit; 

end  {  KeyHlt=  f3) 

until  ( ac 1 1 vemode =ex  1 1  ) 


end;  (  - ActlveControl  -  1 
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t 


begin  (  -  StatusAndCommand  -  ) 

repeat 

S  t  a  t>i  s  An  '3  Coir  re  a  rid  Screen; 

OetKey  (  1  ' , »o3» , mode  2  )  ; 

If  (  ord  (mode)  =  27  )  and  (  ord  (mode2)  =  59  )  then 

begin 

clrbox2  (xl(yl,x2,y2); 

ClosedLoopControl ; 

end; 

until  (  mode  in  t'Q'/'q'l  ); 

end;  l  -  StatusAndCommand  -  1 


procedure  Initial lzeZeroDlglt a ISIgna lout; 

(  Tills  procedure  MUST  be  executed  as  the  first  procedure  called  in  the  main 
program  to  insure  a  zero  signal  out  on  the  2  output  channels.  Otherwise  the 
DT  2  8  0 1 - A  board  defaults  to  a  minimum  full  scale  output.  ) 

const 


dlgltalchanO  =  0; 

digits lchanl  =  1 ; 

dlgltalcommandboard  -•  1; 

var 

status , 

d lg 1 ta Ida ta va 1 ue  :  Integer; 


begin 

d 1 gi t a  1  da t a va 1 ue  : =  2048;  i  This  will  be  converted  to  an  equivalent 

zero  analog  signal  out  on  a  12  bit 
resolution  converter  like  DT  2801-A.  ) 

status  initialize; 

status  seloctboard  (  dlgltalcommandboard  ); 

status  :=  dacvalue  (  d 1 g 1 1 a  1 chanO ,  d i g  1 1 a  Ida ta va 1 ue  ); 
status  :=  dacvalue  (  d 1 g 1 ta lchanl ,  d lg 1 ta Ida ta va 1 ue  ); 
status  :=  terminate; 
end ; 

procedure  DeactlvateADBoardAndExltProgram; 

(  Tills  procedure  deactivates  the  DT  2801-A  board  and  presents  an  exit  screen.) 
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t 

status  :  integer; 

begin  l  -  Deact 1 vateADBoardAndExl tProgram  -  ) 

status  :=  terminate;  y 

clrbox2  ( xl , y 1 , x2 , y 2  )  ; 

boxprint  ( y 1 H 0 , xl , x2 , ’ TUI S  CONCLUDES  YOUR  AUV  AUTOP I LOTTI NG  SESSION  ,  BYE'); 


end;  {  -  Deact 1 vateADBoa r d AndEx 1 tPr ogr am  -  ) 


BEGIN  I  - ; -  MAIN  PROGRAM  -  ) 

InltlallzeZeroDlgltalSlgnalOut; 

clrscr  ; 

repeat 

MalnMenu  (  option  ); 

If  (  option  In  t'R'j’r'l)  then 
begin 

repeat 

begin 

Status AndCommand  (  controlmode  ); 
end ; 

until  (  controlmode  In  ['q'/'Q')); 

end ;  > 

until  (  option  In  t'Q'/'q'l); 

Deact l va t eADBoard AndEx 1 tProgram; 

END.  I  - MAIN  PROGRAM  - - -  ) 
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